


#include <stdio.h>
#include <unistd.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include "qelib.h"
#include "gev.h"
#include <string.h>


#define GEV_LOGNAME         "gev-dev"
#define gevdev_debug(...)   qelog_debug(GEV_LOGNAME,   __VA_ARGS__)
#define gevdev_info(...)    qelog_info(GEV_LOGNAME,    __VA_ARGS__)
#define gevdev_notice(...)  qelog_notice(GEV_LOGNAME,  __VA_ARGS__)
#define gevdev_warning(...) qelog_warning(GEV_LOGNAME, __VA_ARGS__)
#define gevdev_error(...)   qelog_error(GEV_LOGNAME,   __VA_ARGS__)


typedef struct gev_regs {
    qe_u32 curr_ip;
    qe_u32 curr_sm;
    qe_u32 curr_gw;
} gev_regs_t;

typedef struct gev_device_ctx {
    int fd;
    qe_workqueue *wq;
    qe_gbuf *rxbuf;
} gev_dev_ctx_t;

static qe_ret get_curr_ifinfo(const char *interface, qe_u32 *ip, qe_u32 *submask, qe_u32 *gw)
{
    int sockfd;
    struct ifreq ifr;
    struct sockaddr_in *addr;

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    strcpy(ifr.ifr_name, interface);
    
    ioctl(sockfd, SIOCGIFADDR, &ifr);
    addr = (struct sockaddr_in *)&(ifr.ifr_addr);
    *ip = addr->sin_addr.s_addr;

    ioctl(sockfd, SIOCGIFNETMASK, &ifr);
    addr = (struct sockaddr_in *)&(ifr.ifr_netmask);
    *submask = addr->sin_addr.s_addr;
    
    ioctl(sockfd, SIOCGIFDSTADDR, &ifr);
    addr = (struct sockaddr_in *)&(ifr.ifr_dstaddr);
    *gw = addr->sin_addr.s_addr;

    close(sockfd);
}

static qe_ret work_device_discover(qe_workqueue *queue, struct qe_worker *worker)
{
    gevdev_debug("work device discover");
    gvcp_cmd_header_t *cmd;
    gvcp_discovery_ack_t ack;
    struct sockaddr_in app_addr;
    socklen_t app_addr_len = sizeof(app_addr);
    gev_dev_ctx_t *ctx = worker->data;
    int n = recvfrom(ctx->fd, ctx->rxbuf->p, ctx->rxbuf->nbytes, 0, 
        (struct sockaddr *)&app_addr, &app_addr_len);
    if (n > 0) {
        gevdev_debug("app addr %p:%d", app_addr.sin_addr.s_addr, app_addr.sin_port);
        cmd = ctx->rxbuf->p;
        if (cmd->command == GEV_DISCOVERY_CMD) {
            gevdev_info("receive discovery cmd");
            ack.head.ack_id = cmd->req_id;
            ack.head.answer = GEV_DISCOVERY_ACK;
            ack.head.status = GEV_STATUS_SUCCESS;
            ack.head.length = sizeof(ack) - sizeof(gvcp_cmd_header_t);
            ack.spec_version_major = 1;
            ack.spec_version_minor = 0;
            sprintf(ack.device_version, "V1.0Patch");
            sprintf(ack.manufacturer_name, "WSCT");
            sprintf(ack.manufacturer_specific_information, "GEVT");
            if (sendto(ctx->fd, &ack, sizeof(ack), 0, 
                (struct sockaddr *)&app_addr, sizeof(app_addr)) < 0) {
                gevdev_error("Send ack error");
            } else {
                gevdev_info("send discovery ack");
            }
        }
    }

    return qe_workqueue_schedule(queue, work_device_discover, 10000, worker->data);
}

int main(int argc, char *argv[])
{
    gev_regs_t regs;

    qelog_init(QELOG_DEBUG, QELOG_HMS | QELOG_DM | QELOG_LV | QELOG_CL);

    gevdev_info("Gev Dev Start");

    qe_memset(&regs, 0x0, sizeof(regs));

    /* get ipaddr and set regs */
    get_curr_ifinfo("eth0", &regs.curr_ip, &regs.curr_sm, &regs.curr_gw);

    gev_dev_ctx_t *ctx = qe_malloc(sizeof(gev_dev_ctx_t));

    ctx->wq = qe_workqueue_new(32);

    ctx->fd = socket(AF_INET, SOCK_DGRAM, 0);

    struct sockaddr_in serverAddress;
    serverAddress.sin_family = AF_INET;
    serverAddress.sin_port = htons(GVCP_PORT);
    serverAddress.sin_addr.s_addr = inet_addr("127.0.0.1");;
    bind(ctx->fd, (struct sockaddr*)&serverAddress, sizeof(serverAddress));


    ctx->rxbuf = qe_gbuf_new(4096);

    qe_workqueue_schedule(ctx->wq, work_device_discover, 1, ctx);

    gevdev_debug("Gev Dev loop");

    do {
        qe_workqueue_service(ctx->wq, qe_time_ms());
    } while (1);

    return -1;
}